#if ZDCONF_ANT_DIVERSITY == 1
#include "zd1205.h"
#include "zdglobal.h"

#undef ACC_1
#undef ACC_2
#define ACC_1 0
#define ACC_2 1
extern  zd_80211Obj_t dot11Obj;
extern void LockPhyReg(zd_80211Obj_t *pObj);
extern void UnLockPhyReg(zd_80211Obj_t *pObj);


#if 0
void zfRfRxStatistics(struct zd1205_private *macp, u8 rssi, u8 ofdm)
{
    struct zsRfPhy_StatSetting *rf ;

    rf = &macp->RfStatSetting;
    if(ofdm)
    {
        rf->Acc_Num_OFDM++;
        rf->Acc_SQ_OFDM+=rssi+10;
        //printk("OFDM SignalQuality = %d, str=%d\n", t->SignalQuality+10, addInfo->Tail.Data.SignalStrength1);
    }
    else
    {
        rf->Acc_Num++;
        rf->Acc_SQ+=rssi+10;
        //printk("CCK SignalQuality = %d, str=%d\n", t->SignalQuality+10, addInfo->Tail.Data.SignalStrength1);
    }
        
}
#endif
void SwitchAntenna(struct zd1205_private *macp)
{
    struct zsRfPhy_StatSetting *rf = &macp->RfStatSetting;

    LockPhyReg(&dot11Obj);//zfLockPhy(dev);
    if (rf->CurrentAntenna)
    {// Switch to Antenna 0
        dot11Obj.SetReg(dot11Obj.reg, ZD1205_CR10, 0x89);
        //zfDelayWriteInternalReg(dev, ZM_PHY_CR10, 0x89);
        //zfFlushDelayWrite(dev);
        rf->CurrentAntenna=0;
    }
    else
    {// Switch to Antenna 1
        dot11Obj.SetReg(dot11Obj.reg, ZD1205_CR10, 0x8B);
        //zfDelayWriteInternalReg(dev, ZM_PHY_CR10, 0x8B);
        //zfFlushDelayWrite(dev);

        rf->CurrentAntenna=1;
    }
    UnLockPhyReg(&dot11Obj);
    //zfUnlockPhy(dev);
    return;


}
void zfHp_AntDiversity(struct zd1205_private *macp, u32 tick)
{

    //zmw_get_wlan_dev(dev);
    //struct zsHpPriv* hpPriv = (struct zsHpPriv*)(wd->hpPrivate);
    struct zsRfPhy_StatSetting *rf = &macp->RfStatSetting;
    if(mBssType == AP_BSS)
        return;
    if(rf->AntennaScheme != 2)
        return;

    rf->Ant_MonitorDur1 = 50;
    rf->Ant_MonitorDur2 = 10;
    rf->EnableSwAntennaDiv = 1;
    rf->NiceSQThr = 48;
    rf->NiceSQThr_OFDM = 48;
    rf->MinSamplesInMT2 = 10;

    switch(rf->AccState)
    {
    case ACC_1:
        if ((tick % rf->Ant_MonitorDur1) == 0)
        {
            if (rf->Acc_Num_OFDM)
                rf->Avg1_SQ_OFDM = rf->Acc_SQ_OFDM / rf->Acc_Num_OFDM;
            else
                rf->Avg1_SQ_OFDM = 0;

            if (rf->Acc_Num)
                rf->Avg1_SQ = rf->Acc_SQ / rf->Acc_Num;
            else
                rf->Avg1_SQ = 0;

            //printk("Avg1_SQ_OFDM = %ld(%03ld), Avg1_SQ = %ld(%03ld), CurAnt = %ld\n", rf->Avg1_SQ_OFDM ,rf->Acc_Num_OFDM, rf->Avg1_SQ,rf->Acc_Num, (u32)rf->CurrentAntenna);
            // Higher SQ is better
#if 1
            if ((rf->Avg1_SQ_OFDM==0 && rf->Avg1_SQ==0) ||
                ((rf->Avg1_SQ_OFDM < rf->NiceSQThr_OFDM) && (rf->Avg1_SQ_OFDM > 0))
                || ((rf->Avg1_SQ_OFDM == 0)
                && ((rf->Avg1_SQ < rf->NiceSQThr) && (rf->Avg1_SQ > 0)))
                )//|| (rf->adapterReady != TRUE)) // disconnected
#endif
            {
                SwitchAntenna(macp);
                rf->AccState = ACC_2;
                rf->TimeElapsedInMT2=0;

            }
            rf->Acc_SQ = 0;
            rf->Acc_Num = 0;
            rf->Acc_SQ_OFDM = 0;
            rf->Acc_Num_OFDM = 0;
        }
        break;
    case ACC_2:
        rf->TimeElapsedInMT2++;
        if (rf->Acc_Num_OFDM > rf->MinSamplesInMT2 ||
            rf->Acc_Num > rf->MinSamplesInMT2 ||
            rf->TimeElapsedInMT2 >= rf->Ant_MonitorDur2)

        {
            if (rf->Acc_Num_OFDM)
            {
                rf->Avg2_SQ_OFDM = rf->Acc_SQ_OFDM / rf->Acc_Num_OFDM;
            }
            else
            {
                rf->Avg2_SQ_OFDM = 0;
                if (rf->Acc_Num)
                    rf->Avg2_SQ = rf->Acc_SQ / rf->Acc_Num;
                else
                    rf->Avg2_SQ = 0;
            }
            // Higher SQ is better
            if ((rf->Avg2_SQ_OFDM < rf->Avg1_SQ_OFDM)
                || (((rf->Avg2_SQ_OFDM == 0) && (rf->Avg1_SQ_OFDM == 0))
                && (rf->Avg2_SQ < rf->Avg1_SQ))
                )//|| (rf->adapterReady != TRUE)) // disconnected
            {
                //printk("rf->Avg2_SQ_OFDM < rf->Avg1_SQ_OFDM = %d\n",rf->Avg2_SQ_OFDM < rf->Avg1_SQ_OFDM);
                //printk("rf->Avg2_SQ_OFDM == 0 && rf->Avg1_SQ_OFDM == 0 = %d", (rf->Avg2_SQ_OFDM == 0) && (rf->Avg1_SQ_OFDM == 0));
                //printk("rf->Avg2_SQ < rf->Avg1_SQ = %d\n",rf->Avg2_SQ < rf->Avg1_SQ);
                SwitchAntenna(macp);
            }
            else if ((rf->EnableSwAntennaDiv & BIT_2) == 0)
            { // Stay at this better antenna.
                u32 tmpValue4CR9=0xE0;
                // Switch Tx Antenna when decide to change main antenna.
                if (rf->CurrentAntenna)
                    tmpValue4CR9 |= BIT_2;
                tmpValue4CR9 |= BIT_0;

                LockPhyReg(&dot11Obj);//zfLockPhy(dev);
                //zfDelayWriteInternalReg(dev, ZM_PHY_CR9, tmpValue4CR9);
                dot11Obj.SetReg(dot11Obj.reg, ZD1205_CR9, tmpValue4CR9);
                //zfFlushDelayWrite(dev);
                UnLockPhyReg(&dot11Obj);//zfUnlockPhy(dev);
                //printk("Switch to Antenna : %ld\n", rf->CurrentAntenna);

            }


            rf->AccState = ACC_1;
            rf->Acc_SQ = 0;
            rf->Acc_Num = 0;
            rf->Acc_SQ_OFDM = 0;
            rf->Acc_Num_OFDM = 0;
        }
        break;
    default:
        //printk("babbaba\n");
        break;
    }
}
int zfHp_setAntenna(struct zd1205_private *macp, u8 idx)
{
    struct zsRfPhy_StatSetting *rf = &macp->RfStatSetting;
    u32 tmpValue4CR9=0xE0;

    if((idx > 1) || (rf->AntennaScheme == 2))
    {
        //printk("Invalid Antenna Number\n");
        return -EINVAL;
        
    }
    if(rf->CurrentAntenna == idx)
    {
        //printk("Antenna Unchanged\n");
        return 0;
    }
    else
    {
        SwitchAntenna(macp);
        // Switch Tx Antenna when decide to change main antenna.
        if (rf->CurrentAntenna)
            tmpValue4CR9 |= BIT_2;
        tmpValue4CR9 |= BIT_0;
        LockPhyReg(&dot11Obj);//zfLockPhy(dev);
        dot11Obj.SetReg(dot11Obj.reg, ZD1205_CR9, tmpValue4CR9);
        //zfDelayWriteInternalReg(dev, ZM_PHY_CR9, tmpValue4CR9);
        //zfFlushDelayWrite(dev);
        UnLockPhyReg(&dot11Obj);//zfUnlockPhy(dev);
        //printk("Switch to Antenna : %ld\n", rf->CurrentAntenna);
    }
    return 0;
}
int zfiHp_AntennaScheme(struct zd1205_private *macp, u8 idx)
{
    struct zsRfPhy_StatSetting *rf = &macp->RfStatSetting;
    if(idx > 2)
        return -EINVAL;
    rf->AntennaScheme = idx;
    if(idx != 2)
        zfHp_setAntenna(macp,idx);
    return 0;

}
#undef ACC_1 
#undef ACC_2 
#endif
